#include "Joint.h"

bool JointBase::calytoq(IN double t, IN double* y)
{
	Matrix3d Ai, Aj, Ah0;
	calAh0(Ah0);
	Ai = Ah0 * calDih() * calCiP().transpose();
	Bj->A(Aj);
	//calculate rotation
	if (Body::m_s_rtype == RCORDS::EULERQUATERNION)
	{
		AUX::AtoEQ(Ai, Bi->pos + 3);
	}
	else if (Body::m_s_rtype == RCORDS::EULERANGLE)
		AUX::AtoEA(Ai, Bi->pos + 3);
	else if (Body::m_s_rtype == RCORDS::CARDANANGLE)
		AUX::AtoCA(Ai, Bi->pos + 3);

	//calculate translation
	Vector3d rhoip = rhoi, rhojq = rhoj;
	rhoip = Ai * rhoip;
	rhojq = Aj * rhojq;
	Map<Vector3d> rj(Bj->pos);
	Vector3d ri = rj - rhoip + rhojq + Ah0 * Hi();
	for (int i = 0; i < 3; ++i)
		Bi->pos[i] = ri(i);
	/*for (unsigned int i = 0; i < Body::NC; ++i)
		cout << Bi->pos[i] << " ";
	cout << endl;*/
	return true;
}

bool JointBase::caldytodq(IN double t, IN double* dy)
{
	VectorXd vi;
	Map<VectorXd> y(dyi, DOF());
	Map<VectorXd> vj(Bj->vel, Body::NC);
	MatrixXd Tij = MatrixXd::Identity(6, 6);
	MatrixXd Ui = MatrixXd::Zero(6, DOF());
	Vector3d v1;
	Matrix3d m1;
	Matrix3d Ah0;
	calAh0(Ah0);
	v1 = Bi->Rho(rhoi) - Ah0 * Hi() - Bj->Rho(rhoj);
	AUX::tilde(v1, m1);
	Tij.block<3, 3>(0, 3) = m1;
	v1 = Bi->Rho(rhoi);
	AUX::tilde(v1, m1);
	Ui.block(0, 0, 3, DOF()) = Ah0 * HhT() + m1 * (Ah0 * HoT());
	Ui.block(3, 0, 3, DOF()) = Ah0 * HoT();
	if (Body::m_s_rtype == RCORDS::EULERQUATERNION)
	{//Euler Quaternion
		MatrixXd Kj = MatrixXd::Zero(6 , 7 );
		Kj.block<3, 3>(0, 0).setIdentity();
		MatR3C4 Rj, Ri;
		AUX::R(Bj->pos + 3, Rj);
		AUX::R(Bi->pos + 3, Ri);
		Kj.block<3, 4>(3, 3) = 2 * Rj;
		vi = Tij * Kj * vj + Ui * y;
		for (unsigned int i = 0; i < 3; ++i)
			Bi->vel[i] = vi(i);
		Vector4d lami = 0.5 * Ri.transpose() * vi.segment<3>(3);
		for (unsigned int i = 0; i < 4; ++i)
			Bi->vel[i + 3] = lami[i];
		//AUX::EQVCorrect(Bi->pos + 3,Bi->vel+3, 1e-4, 1e-3, 1);
	}
	else if (Body::m_s_rtype == RCORDS::EULERANGLE)
	{//Euler Angle
		MatrixXd Kj = MatrixXd::Zero(6 , 6 );
		Kj.block<3, 3>(0, 0).setIdentity();
		MatR3CX Krj(3, 3), Kri(3, 3);
		AUX::Kr(Bj->pos + 3, RCORDS::EULERANGLE, Krj);
		AUX::Kr(Bi->pos + 3, RCORDS::EULERANGLE, Kri);
		Kj.block<3, 3>(3, 3) = Krj;
		vi = Tij * Kj * vj + Ui * y;
		for (unsigned int i = 0; i < 3; ++i)
			Bi->vel[i] = vi(i);
		Vector3d lami = Kri.partialPivLu().solve(vi.segment<3>(3));
		for (unsigned int i = 0; i < 3; ++i)
			Bi->vel[i + 3] = lami[i];
	}
	else if (Body::m_s_rtype == RCORDS::CARDANANGLE)
	{//Cardan Angle
		MatrixXd Kj = MatrixXd::Zero(6 , 6 );
		Kj.block<3, 3>(0, 0).setIdentity();
		MatR3CX Krj(3, 3), Kri(3, 3);
		AUX::Kr(Bj->pos + 3, RCORDS::CARDANANGLE, Krj);
		AUX::Kr(Bi->pos + 3, RCORDS::CARDANANGLE, Kri);
		Kj.block<3, 3>(3, 3) = Krj;
		vi = Tij * Kj * vj + Ui * y;
		for (unsigned int i = 0; i < 3; ++i)
			Bi->vel[i] = vi(i);
		Vector3d lami = Kri.partialPivLu().solve(vi.segment<3>(3));
		for (unsigned int i = 0; i < 3; ++i)
			Bi->vel[i + 3] = lami[i];
	}
	return true;
}

bool JointBase::setCiP(IN const Matrix3d& c)
{
	CiP = c;
	return true;
}

bool JointBase::setCjQ(IN const Matrix3d& c)
{
	CjQ = c;
	return true;
}

Vector3d JointBase::YitaPlusZeta()
{
	Matrix3d Ah0;
	calAh0(Ah0);
	Vector3d yz = Ah0 * Yita();
	yz += Bj->AngularVel().cross(Ah0 * Wri());
	return yz;
}

bool JointBase::CalHoT(MatR3CX& hot)
{
	Matrix3d Ah0;
	calAh0(Ah0);
	hot = Ah0 * HoT();
	return true;
}

bool JointBase::CalHhT(MatR3CX& hht)
{
	Matrix3d Ah0;
	calAh0(Ah0);
	hht = Ah0 * HhT();
	return true;
}

bool JointBase::Write(OUT Json::Value& joint)
{
	if (Type() == JointBase::REVOLUTIONAL)
		joint["Type"] = Json::Value("Revolute");
	else if (Type() == JointBase::UNIVERSAL)
		joint["Type"] = Json::Value("Universe");
	else if (Type() == JointBase::SPHERICAL)
		joint["Type"] = Json::Value("Sphere");
	else if (Type() == JointBase::PRISMATIC)
		joint["Type"] = Json::Value("Prism");
	else if (Type() == JointBase::CYLINDRICAL)
		joint["Type"] = Json::Value("Cylinder");
	else if (Type() == JointBase::VIRTUAL)
		joint["Type"] = Json::Value("Virtual");
	else
		throw MBException("There is no such type joint.");
	joint["Bi_Id"] = Json::Value(Bi->id);
	joint["Bj_Id"] = Json::Value(Bj->id);
	for (int i = 0; i < 3; ++i)
	{
		joint["Rhoi"].append(rhoi(i));
		joint["Rhoj"].append(rhoj(i));
	}
	for (unsigned int i = 0; i < DOF() + Bi->nMode(); ++i)
	{
		joint["Position"].append(yi[i]);
		joint["Velocity"].append(dyi[i]);
	}
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			joint["CiP"].append(CiP(i, j));
			joint["CjQ"].append(CjQ(i, j));
		}
	}
	return true;
}

Matrix3d& JointBase::calCiP()
{
	return CiP;
}

Matrix3d& JointBase::calCjQ()
{
	return CjQ;
}

bool JointBase::calAh0(OUT Matrix3d& A)
{
	Bj->A(A);
	A *= calCjQ();
	return true;
}

Vector3d JointBase::Hi() const
{
	Map<VectorXd> y(yi, DOF());
	return HhT() * y;
}

Vector3d JointBase::Wri() const
{
	Map<VectorXd> dy(dyi, DOF());
	return HoT() * dy;
}

Vector3d JointBase::Vri() const
{
	Map<VectorXd> dy(dyi, DOF());
	return HhT() * dy;
}

JointBase::JointBase(IN Body* Bi_ptr, IN Body* Bj_ptr, IN const Vector3d& rho_i, IN const Vector3d& rho_j)
{
	Bi = Bi_ptr;
	Bj = Bj_ptr;
	rhoi = rho_i;
	rhoj = rho_j;
	CiP.setIdentity();
	CjQ.setIdentity();
	Dih.setIdentity();
}

JointBase::~JointBase()
{

}

bool JointBase::operator<(IN const JointBase& other) const
{
	return (*Bi) < (*other.Bi);
}

int JointBase::GetBiId() const
{
	return Bi->GetID();
}

int JointBase::GetBjId() const
{
	return Bj->GetID();
}

MatR3CX Revolute::HhT() const
{
	return Vector3d(0, 0, 0);
}

MatR3CX Revolute::HoT() const
{
	return Vector3d(1, 0, 0);
}

Vector3d Revolute::Yita() const
{
	return Vector3d(0, 0, 0);
}

Matrix3d& Revolute::calDih()
{
	// Assume rotate along x-axis.
	Dih.setIdentity();
	Dih(1, 1) = cos(yi[0]);
	Dih(1, 2) = -sin(yi[0]);
	Dih(2, 1) = -Dih(1, 2);
	Dih(2, 2) = Dih(1, 1);
	return Dih;
}

Revolute::Revolute(IN Body* Bi_ptr, IN Body* Bj_ptr, IN Vector3d& rho_i, IN Vector3d& rho_j):
	JointBase(Bi_ptr, Bj_ptr, rho_i, rho_j)
{
}

inline unsigned int Revolute::DOF() const
{
	return 1;
}

inline unsigned int Revolute::Type() const
{
	return JointBase::REVOLUTIONAL;
}

MatR3CX Universe::HhT() const
{
	return MatR3CX(3,2);
}

MatR3CX Universe::HoT() const
{
	MatR3CX m = MatR3CX::Zero(3, 2);
	m(0, 0) = 1;
	m(1, 1) = cos(yi[0]);
	m(2, 1) = sin(yi[0]);
	return m;
}

Vector3d Universe::Yita() const
{
	Vector3d v(0, 0, 0);
	v(1) = -sin(yi[0]) * dyi[0] * dyi[1];
	v(2) = cos(yi[0]) * dyi[0] * dyi[1];
	return v;
}

Matrix3d& Universe::calDih()
{
	//assume rotate along x-axis and y-axis
	double s1 = sin(yi[0]), c1 = cos(yi[0]);
	double s2 = sin(yi[1]), c2 = cos(yi[1]);
	Dih(0, 0) = c2;
	Dih(0, 1) = 0;
	Dih(0, 2) = s2;
	Dih(1, 0) = s1 * s2;
	Dih(1, 1) = c1;
	Dih(1, 2) = -s1 * c2;
	Dih(2, 0) = -c1 * s2;
	Dih(2, 1) = s1;
	Dih(2, 2) = c1 * c2;
	return Dih;
}

Universe::Universe(IN Body* Bi_ptr, IN Body* Bj_ptr, IN Vector3d& rho_i, IN Vector3d& rho_j):
	JointBase(Bi_ptr, Bj_ptr, rho_i, rho_j)
{
}

inline unsigned int Universe::DOF() const
{
	return 2;
}

inline unsigned int Universe::Type() const
{
	return JointBase::UNIVERSAL;
}

MatR3CX Virtual::HhT() const
{
	MatR3CX hht = MatR3CX::Zero(3, 6);
	hht.block<3, 3>(0, 0).setIdentity();
	return hht;
}

MatR3CX Virtual::HoT() const
{
	MatR3CX hot = MatR3CX::Zero(3, 6);
	double s1 = sin(yi[3]), c1 = cos(yi[3]), s2 = sin(yi[4]), c2 = cos(yi[4]);
	hot(0, 3) = 1;
	hot(0, 5) = s2;
	hot(1, 4) = c1;
	hot(1, 5) = -c2 * s1;
	hot(2, 4) = s1;
	hot(2, 5) = c2 * c1;
	return hot;
}

Vector3d Virtual::Yita() const
{
	Vector3d ans(0, 0, 0);
	double s1 = sin(yi[3]), c1 = cos(yi[3]);
	double s2 = sin(yi[4]), c2 = cos(yi[4]);
	double dq1 = dyi[3], dq2 = dyi[4], dq3 = dyi[5];
	ans(0) = c1 * dq2 * dq3;
	ans(1) = -s1 * dq1 * dq2 - c1 * c2 * dq1 * dq3 + s1 * s2 * dq2 * dq3;
	ans(2) = c1 * dq1 * dq2 - s1 * c2 * dq1 * dq3 - c1 * s2 * dq2 * dq3;
	return ans;
}

Matrix3d& Virtual::calDih()
{
	double s1 = sin(yi[3]), c1 = cos(yi[3]);
	double s2 = sin(yi[4]), c2 = cos(yi[4]);
	double s3 = sin(yi[5]), c3 = cos(yi[5]);
	Dih(0, 0) = c2 * c3;
	Dih(0, 1) = -c2 * s3;
	Dih(0, 2) = s2;
	Dih(1, 0) = s1 * s2 * c3 + c1 * s3;
	Dih(1, 1) = -s1 * s2 * s3 + c1 * c3;
	Dih(1, 2) = -s1 * c2;
	Dih(2, 0) = -c1 * s2 * c3 + s1 * s3;
	Dih(2, 1) = c1 * s2 * s3 + s1 * c3;
	Dih(2, 2) = c1 * c2;
	return Dih;
}

Virtual::Virtual(IN Body* Bi_ptr, IN Body* Bj_ptr, IN Vector3d& rho_i, IN Vector3d& rho_j):
	JointBase(Bi_ptr, Bj_ptr, rho_i, rho_j)
{
}

inline unsigned int Virtual::DOF() const
{
	return 6;
}

inline unsigned int Virtual::Type() const
{
	return JointBase::VIRTUAL;
}

MatR3CX Sphere::HhT() const
{
	return MatR3CX::Zero(3, 3);
}

MatR3CX Sphere::HoT() const
{
	MatR3CX hot = MatR3CX::Zero(3, 3);
	double s1 = sin(yi[0]), c1 = cos(yi[0]);
	double s2 = sin(yi[1]), c2 = cos(yi[1]);
	hot(0, 0) = 1;
	hot(0, 2) = s2;
	hot(1, 1) = c1;
	hot(1, 2) = -c2 * s1;
	hot(2, 1) = s1;
	hot(2, 2) = c2 * c1;
	return hot;
}

Vector3d Sphere::Yita() const
{
	Vector3d ans(0, 0, 0);
	double s1 = sin(yi[0]), c1 = cos(yi[0]);
	double s2 = sin(yi[1]), c2 = cos(yi[1]);
	double dq1 = dyi[0], dq2 = dyi[1], dq3 = dyi[2];
	ans(0) = c1 * dq2 * dq3;
	ans(1) = -s1 * dq1 * dq2 - c1 * c2 * dq1 * dq3 + s1 * s2 * dq2 * dq3;
	ans(2) = c1 * dq1 * dq2 - s1 * c2 * dq1 * dq3 - c1 * s2 * dq2 * dq3;
	return ans;
}

Matrix3d& Sphere::calDih()
{
	double s1 = sin(yi[0]), c1 = cos(yi[0]);
	double s2 = sin(yi[1]), c2 = cos(yi[1]);
	double s3 = sin(yi[2]), c3 = cos(yi[2]);
	Dih(0, 0) = c2 * c3;
	Dih(0, 1) = -c2 * s3;
	Dih(0, 2) = s2;
	Dih(1, 0) = s1 * s2 * c3 + c1 * s3;
	Dih(1, 1) = -s1 * s2 * s3 + c1 * c3;
	Dih(1, 2) = -s1 * c2;
	Dih(2, 0) = -c1 * s2 * c3 + s1 * s3;
	Dih(2, 1) = c1 * s2 * s3 + s1 * c3;
	Dih(2, 2) = c1 * c2;
	return Dih;
}

Sphere::Sphere(IN Body* Bi_ptr, IN Body* Bj_ptr, IN Vector3d& rho_i, IN Vector3d& rho_j):
	JointBase(Bi_ptr, Bj_ptr, rho_i, rho_j)
{
}

inline unsigned int Sphere::DOF() const
{
	return 3;
}

inline unsigned int Sphere::Type() const
{
	return JointBase::SPHERICAL;
}

MatR3CX Prism::HhT() const
{
	return Vector3d(1, 0, 0);
}

MatR3CX Prism::HoT() const
{
	return Vector3d(0, 0, 0);
}

Vector3d Prism::Yita() const
{
	return Vector3d(0, 0, 0);
}

Matrix3d& Prism::calDih()
{
	Dih.setIdentity();
	return Dih;
}

Prism::Prism(IN Body* Bi_ptr, IN Body* Bj_ptr, IN Vector3d& rho_i, IN Vector3d& rho_j):
	JointBase(Bi_ptr, Bj_ptr, rho_i, rho_j)
{
}

inline unsigned int Prism::DOF() const
{
	return 1;
}

inline unsigned int Prism::Type() const
{
	return JointBase::PRISMATIC;
}

MatR3CX Cylinder::HhT() const
{
	MatR3CX hht = MatR3CX::Zero(3, 2);
	hht(0, 0) = 1;
	return hht;
}

MatR3CX Cylinder::HoT() const
{
	MatR3CX hot = MatR3CX::Zero(3, 2);
	hot(0, 1) = 1;
	return hot;
}

Vector3d Cylinder::Yita() const
{
	return Vector3d(0, 0, 0);
}

Matrix3d& Cylinder::calDih()
{
	double s1 = sin(yi[1]), c1 = cos(yi[1]);
	Dih(1, 1) = c1;
	Dih(1, 2) = -s1;
	Dih(2, 1) = s1;
	Dih(2, 2) = c1;
	return Dih;
}

Cylinder::Cylinder(IN Body* Bi_ptr, IN Body* Bj_ptr, IN Vector3d& rho_i, IN Vector3d& rho_j):
	JointBase(Bi_ptr, Bj_ptr, rho_i, rho_j)
{
}

inline unsigned int Cylinder::DOF() const
{
	return 2;
}

inline unsigned int Cylinder::Type() const
{
	return JointBase::CYLINDRICAL;
}
